/*
  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

  This software is provided 'as-is', without any express or implied
  warranty.  In no event will the authors be held liable for any damages
  arising from the use of this software.

  Permission is granted to anyone to use this software for any purpose,
  including commercial applications, and to alter it and redistribute it
  freely, subject to the following restrictions:

  1. The origin of this software must not be misrepresented; you must not
     claim that you wrote the original software. If you use this software
     in a product, an acknowledgment in the product documentation would be
     appreciated but is not required.
  2. Altered source versions must be plainly marked as such, and must not be
     misrepresented as being the original software.
  3. This notice may not be removed or altered from any source distribution.
*/

using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.BulletX.Dynamics
{
	[Serializable]
	public class RigidBody : CollisionObject
	{
		private static float _linearAirDamping = 1;
		//'temporarily' global variables
		private static float _rigidBodyDeactivationTime = 2;
		private static bool _disableDeactivation = false;

		private static float linearSleepingThreshold = 0.8f;
		private static float angularSleepingThreshold = 1.0f;
		private static int _uniqueId = 0;

		public Matrix InvInertiaTensorWorld;
		public Vector3 _linearVelocity;
		public Vector3 _angularVelocity;
		public float InverseMass;
		public float AngularFactor;

		private Vector3 _gravity;
		public Vector3 InvInertiaDiagLocal;
		private Vector3 _totalForce;
		private Vector3 _totalTorque;

		private float _linearDamping;
		private float _angularDamping;

		//m_optionalMotionState allows to automatic synchronize the world transform for active objects
		private MotionState _optionalMotionState;

		//for experimental overriding of friction/contact solver func
		public ContactSolverType ContactSolverType;
		public ContactSolverType FrictionSolverType;

		public int DebugBodyID;

		//Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
		//doesn't work very well yet (value 0 disabled this damping)
		//note there this influences deactivation thresholds!
		private float _clippedAngvelThresholdSqr = 0.01f;
		private float _clippedLinearThresholdSqr = 0.01f;

		private float _jitterVelocityDampingFactor = 0.7f;

		//keep track of typed constraints referencing this rigid body
		private List<TypedConstraint> _constraintRefs = new List<TypedConstraint>();

		private float _linearSleepingThreshold;
		private float _angularSleepingThreshold;

		public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape)
			: this(mass, motionState, collisionShape, new Vector3(0, 0, 0), 0.0f, 0.0f, 0.5f, 0.0f) { }

		public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution)
		{
			_linearSleepingThreshold = linearSleepingThreshold;
			_angularSleepingThreshold = angularSleepingThreshold;

			_optionalMotionState = motionState;
			AngularFactor = 1;
			_angularDamping = 0.5f;

			if (motionState != null)
			{
				motionState.GetWorldTransform(out WorldTransform);
			}
			else
			{
				WorldTransform = Matrix.Identity;
			}

			InterpolationWorldTransform = WorldTransform;
			InterpolationLinearVelocity = new Vector3();
			InterpolationAngularVelocity = new Vector3();

			//moved to btCollisionObject
			Friction = friction;
			Restitution = restitution;

			CollisionShape = collisionShape;
			DebugBodyID = UniqueID++;

			//m_internalOwner is to allow upcasting from collision object to rigid body
			Owner = this;

			SetMassProps(mass, localInertia);
			SetDamping(linearDamping, angularDamping);
			UpdateInertiaTensor();
		}

		//is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
		public bool IsInWorld { get { return Broadphase != null; } }

		public Vector3 Gravity
		{
			get { return _gravity; }
			set
			{
				if (InverseMass != 0.0f)
				{
					_gravity = value * (1.0f / InverseMass);
				}
			}
		}
		public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } }
		public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } }

		public Matrix CenterOfMassTransform { get { return this.WorldTransform; } }

		public void SetCenterOfMassTransform( Matrix value )
		{
			if (IsStaticOrKinematicObject)
			{
				InterpolationWorldTransform = WorldTransform;
			}
			else
			{
				InterpolationWorldTransform = value;
			}
			InterpolationLinearVelocity = LinearVelocity;
			InterpolationAngularVelocity = AngularVelocity;
			WorldTransform = value;
			UpdateInertiaTensor();
		}

		public Vector3 LinearVelocity
		{
			get { return _linearVelocity; }
			set
			{
				if (CollisionFlags == CollisionOptions.StaticObject)
					throw new BulletException("Static objects can't have linear velocity!");
				_linearVelocity = value;
			}
		}

		public Vector3 AngularVelocity
		{
			get { return _angularVelocity; }
			set
			{
				if (CollisionFlags == CollisionOptions.StaticObject)
					throw new BulletException("Static objects can't have angular velocity!");
				_angularVelocity = value;
			}
		}

		//MotionState allows to automatic synchronize the world transform for active objects
		public MotionState MotionState
		{
			get { return _optionalMotionState; }
			set
			{
				_optionalMotionState = value;
				if (_optionalMotionState != null)
					value.GetWorldTransform(out WorldTransform);
			}
		}

		public int ConstrainRefCount { get { return _constraintRefs.Count; } }

		public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } }
		public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } }
		public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } }
		public static float LinearSleepingThreshold { get { return linearSleepingThreshold; } set { linearSleepingThreshold = value; } }
		public static float AngularSleepingThreshold { get { return angularSleepingThreshold; } set { angularSleepingThreshold = value; } }
		public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } }

		public void SetSleepingThresholds(float linear, float angular)
		{
			_linearSleepingThreshold = linear;
			_angularSleepingThreshold = angular;
		}

		public TypedConstraint GetConstraintRef(int index)
		{
			return _constraintRefs[index];
		}

		public void ProceedToTransform(Matrix newTrans)
		{
			this.SetCenterOfMassTransform( newTrans );
		}

		//to keep collision detection and dynamics separate we don't store a rigidbody pointer
		//but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
#warning this is stupid
		public static RigidBody Upcast(CollisionObject colObj)
		{
			return colObj.Owner as RigidBody;
		}

		// continuous collision detection needs prediction
		public void PredictIntegratedTransform(float step, ref Matrix predictedTransform)
		{
			if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) &&
				(_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr))
			{
				_angularVelocity *= _jitterVelocityDampingFactor;
				_linearVelocity *= _jitterVelocityDampingFactor;
			}

			TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform);
		}

		public void SaveKinematicState(float step)
		{
			//todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
			if (step != 0)
			{
				//if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
				if (MotionState != null)
					MotionState.GetWorldTransform(out WorldTransform);

				TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);
				InterpolationLinearVelocity = _linearVelocity;
				InterpolationAngularVelocity = _angularVelocity;
				InterpolationWorldTransform = WorldTransform;
			}
		}

		public void ApplyForces(float step)
		{
			if (IsStaticOrKinematicObject)
				return;

			ApplyCentralForce(_gravity);

			_linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); 
			_angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); 

			float speed = _linearVelocity.Length();
			if (speed < _linearDamping)
			{
				float dampVel = 0.005f;
				if (speed > dampVel)
				{
					Vector3 dir = _linearVelocity;
					dir.Normalize();
					_linearVelocity -= dir * dampVel;
				}
				else
				{
					_linearVelocity = new Vector3();
				}
			}

			float angSpeed = _angularVelocity.Length();
			if (angSpeed < _angularDamping)
			{
				float angDampVel = 0.005f;
				if (angSpeed > angDampVel)
				{
					Vector3 dir = _angularVelocity;
					dir.Normalize();
					_angularVelocity -= dir * angDampVel;
				}
				else
				{
					_angularVelocity = new Vector3();
				}
			}
		}

		public void SetDamping(float linDamping, float angDamping)
		{
			_linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); 
			_angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); 
		}

		public void SetMassProps(float mass, Vector3 inertia)
		{
			if (mass == 0)
			{
				CollisionFlags |= CollisionOptions.StaticObject;
				InverseMass = 0;
			}
			else
			{
				CollisionFlags &= (~CollisionOptions.StaticObject);
				InverseMass = 1.0f / mass;
			}

			InvInertiaDiagLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f,
								   inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f,
								   inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f);
		}

		public void IntegrateVelocities(float step)
		{
			if (IsStaticOrKinematicObject)
				return;

			_linearVelocity += _totalForce * (InverseMass * step);
			_angularVelocity += Vector3.TransformNormal(_totalTorque, InvInertiaTensorWorld) * step;

			float MAX_ANGVEL = Microsoft.Xna.Framework.MathHelper.PiOver2;
			/// clamp angular velocity. collision calculations will fail on higher angular velocities	
			float angvel = _angularVelocity.Length();
			if (angvel * step > MAX_ANGVEL)
			{
				_angularVelocity *= (MAX_ANGVEL / step) / angvel;
			}

			ClearForces();
		}

		public void ApplyCentralForce(Vector3 force)
		{
			_totalForce += force;
		}

		public void ApplyTorque(Vector3 torque)
		{
			_totalTorque += torque;
		}

		public void ApplyForce(Vector3 force, Vector3 rel_pos)
		{
			ApplyCentralForce(force);
			ApplyTorque(Vector3.Cross(rel_pos, force));
		}

		public void ApplyCentralImpulse(Vector3 impulse)
		{
			_linearVelocity += impulse * InverseMass;
		}

		public void ApplyTorqueImpulse(Vector3 torque)
		{
			_angularVelocity += Vector3.TransformNormal(torque, InvInertiaTensorWorld);
		}

		public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos)
		{
			if (InverseMass != 0)
			{
				ApplyCentralImpulse(impulse);
				if (AngularFactor != 0)
					ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * AngularFactor);
			}
		}

		public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
		{
			if (InverseMass != 0)
			{
				_linearVelocity += linearComponent * impulseMagnitude;
				if (AngularFactor != 0)
					_angularVelocity += angularComponent * impulseMagnitude * AngularFactor;
			}
		}

		public void ClearForces()
		{
			_totalForce = new Vector3();
			_totalTorque = new Vector3();
		}

		public void UpdateInertiaTensor()
		{
			Matrix temp = WorldTransform;
			temp.Translation = Vector3.Zero;
			Matrix scaled = MatrixOperations.Scaled( ref WorldTransform, ref InvInertiaDiagLocal );
			Matrix transposed = Matrix.Transpose( temp );
			InvInertiaTensorWorld = MatrixOperations.Multiply(ref scaled, ref transposed);
		}

		public Vector3 GetVelocityInLocalPoint(Vector3 relPos)
		{
			//we also calculate lin/ang velocity for kinematic objects
			return _linearVelocity + Vector3.Cross(_angularVelocity, relPos);

			//for kinematic objects, we could also use use:
			//		return 	(m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
		}

		public void Translate(Vector3 v)
		{
			Matrix m = WorldTransform;
			m.Translation += v;
			WorldTransform = m;
		}

		public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax)
		{
			CollisionShape.GetAabb( ref WorldTransform, out aabbMin, out aabbMax );
		}

		public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal)
		{
			Vector3 r0 = pos - CenterOfMassPosition;
			Vector3 c0 = Vector3.Cross(r0, normal);
			Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0);

			return InverseMass + Vector3.Dot(normal, vec);

		}

		public float ComputeAngularImpulseDenominator(Vector3 axis)
		{
			Vector3 vec;
			Vector3.TransformNormal( ref axis, ref InvInertiaTensorWorld, out vec );
			float result;
			Vector3.Dot( ref axis, ref vec, out result );
			return result;
		}

		public void UpdateDeactivation(float timeStep)
		{
			if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))
				return;

			if ((LinearVelocity.LengthSquared() < _linearSleepingThreshold * _linearSleepingThreshold) &&
				(AngularVelocity.LengthSquared() < _angularSleepingThreshold * _angularSleepingThreshold))
			{
				DeactivationTime += timeStep;
			}
			else
			{
				DeactivationTime = 0;
				this.SetActivationState( ActivationState.Nothing );
			}

		}

		public bool WantsSleeping()
		{

			if (ActivationState == ActivationState.DisableDeactivation)
				return false;

			//disable deactivation
			if (DisableDeactivation || (RigidBodyDeactivationTime == 0))
				return false;

			if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation))
				return true;

			if (DeactivationTime > RigidBodyDeactivationTime)
			{
				return true;
			}
			return false;
		}

		protected override bool CheckCollideWithOverride(CollisionObject co)
		{
			RigidBody otherRb = RigidBody.Upcast(co);
			if (otherRb == null)
				return true;

			for (int i = 0; i < _constraintRefs.Count; i++)
			{
				TypedConstraint c = _constraintRefs[i];
				if (c.RigidBodyA == otherRb || c.RigidBodyB == otherRb)
					return false;
			}

			return true;
		}

		public void AddConstraintRef(TypedConstraint c)
		{
			if (!_constraintRefs.Contains(c))
				_constraintRefs.Add(c);

			CheckCollide = true;
		}

		public void RemoveConstraintRef(TypedConstraint c)
		{
			_constraintRefs.Remove(c);
			CheckCollide = _constraintRefs.Count > 0;
		}
	}
}
